#include "safety_event.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "safety/io/safeio.h"
#include "log/zuclog.h"
#include "rtdev/safetyboard/scbitf.h"
#include "rtbus/rtbus.h"

#define SAFETY_INFO(fmt, ...) zuclog_info("SAFETY_ACTION", fmt, ##__VA_ARGS__)
#define SAFETY_ERR(fmt, ...) zuclog_error("SAFETY_ACTION", fmt, ##__VA_ARGS__)
#define SAFETY_DEBUG(fmt, ...) zuclog_debug("SAFETY_ACTION", fmt, ##__VA_ARGS__)

using safety::SafetyAction;
using safety::SafetyEventWatch;

namespace safety {
class EStopIO : public IOState
{
public:
    bool check_safety_io() override { return safety::SafeIOMgr::instance().is_estop(); }
};
// Cat.0 停止， 根据配置是否暂停触发 motion cat0 停止 或 cat2 停止（暂停）；
// 1s 后，通知SCB断电
class Cat0Action : public SafetyAction
{
public:
    const double POWER_OFF_DELAY_SEC = 1.0;
    Cat0Action() : SafetyAction()
    {
        io_state_.clear();
        io_state_.push_back(std::make_shared<EStopIO>());
    }

    // 上升沿事件
    int rising_event() override
    {
        SAFETY_DEBUG("Cat.0 Signal come in!");
        for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
        {
            if (pause_or_stop_ == Action::STOP)
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->motion_abort();
            }
            else
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->pause();
            }
        }
        if (count_down_ == 0)
        {
            count_down_ = (int)(POWER_OFF_DELAY_SEC * SafetyEventWatch::WatchFrq());
        }
        return 0;
    }
    // 下降沿事件
    int falling_event() override
    {
        SAFETY_DEBUG("ESTOP recover!");
        return 0;
    }
    // 上升或下降沿触发后的计时处理事件
    int watch_count_down() override
    {
        if (count_down_)
        {
            count_down_--;
            if (!count_down_)
            {
                // 断电 直接操作还是发送cmd指令呢？
                for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
                {
                    auto rob = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
                    rob->do_power(false);  // 切换至booting状态
                    auto config = rob->get_robot_active_config();
                    SAFETY_DEBUG("Try to deactive bus %s", config.robot_axis_cfg.bus_chn.c_str());
                    //FIXME：换成非阻塞的
                    rtbus::BusMgr::get()->bus(config.robot_axis_cfg.bus_chn)->deactive();
                    return rtdev::RtDevMgr::instance().get_scb()->rob_power()->set_power(false, i);
                }
            }
        }
        return 0;
    }
    // 所有关联信号为安全状态，才安全
    int check_not_safe(bool& state) override
    {
        state = false;
        for (auto io : io_state_) { state = state || io->check_safety_io(); }
        return 0;
    }
};

// Cat.1 停止， 根据配置是否暂停触发 或 cat2 停止（暂停）；
// standby后，或超时后，下使能
class Cat1Action : public SafetyAction
{
public:
    const double ENABLE_OFF_DELAY_SEC = 2.0;
    Cat1Action() : SafetyAction() { io_state_.clear(); }

    // 上升沿事件
    int rising_event() override
    {
        SAFETY_DEBUG("Cat.1 signal come in!");
        for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
        {
            if (pause_or_stop_ == Action::STOP)
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->motion_abort();
            }
            else
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->pause();
            }
        }
        if (count_down_ == 0)
        {
            count_down_ = (int)(ENABLE_OFF_DELAY_SEC * SafetyEventWatch::WatchFrq());
        }
        return 0;
    }
    // 下降沿事件
    int falling_event() override
    {
        SAFETY_DEBUG("Cat.1 signal recover!");
        return 0;
    }
    // 上升或下降沿触发后的计时处理事件
    int watch_count_down() override
    {
        if (count_down_)
        {
            count_down_--;
            if (!count_down_)
            {
                for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
                {
                    auto rob = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
                    rob->do_enable(false);  // 强制下使能
                }
            }
            else
            {
                for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
                {
                    auto rob = axisgroup::AxisGroupMgr::instance().get_axisgroup(i);
                    if (rob->is_inpos())  // 到位下使能
                    {
                        rob->do_enable(false);
                    }
                }
            }
        }
        return 0;
    }

    int check_not_safe(bool& state) override
    {
        state = false;
        for (auto io : io_state_) { state = state || io->check_safety_io(); }
        return 0;
    }
};

class PStopIO : public IOState
{
public:
    bool check_safety_io() override { return safety::SafeIOMgr::instance().is_protective_stop(); }
};
// Cat.2 停止， 根据配置是否暂停触发 或 cat2 停止（暂停）；
// 1s 后，通知SCB断电
class Cat2Action : public SafetyAction
{
public:
    Cat2Action() : SafetyAction()
    {
        io_state_.clear();
        io_state_.push_back(std::make_shared<PStopIO>());
    }

    // 上升沿事件
    int rising_event() override
    {
        SAFETY_DEBUG("Cat.2 signal come in!");
        for (size_t i = 0; i < axisgroup::AxisGroupMgr::instance().group_num(); i++)
        {
            if (pause_or_stop_ == Action::STOP)
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->motion_abort();
            }
            else
            {
                axisgroup::AxisGroupMgr::instance().get_axisgroup(i)->pause();
            }
        }
        return 0;
    }
    // 下降沿事件
    int falling_event() override
    {
        SAFETY_DEBUG("Cat.2 signal recover!");
        return 0;
    }
    // 上升或下降沿触发后的计时处理事件
    int watch_count_down() override { return 0; }

    int check_not_safe(bool& state) override
    {
        state = false;
        for (auto io : io_state_) { state = state || io->check_safety_io(); }
        return 0;
    }
};

}  // namespace safety

/***************************************************************************************************
 * SafetyAction 监视状态，执行动作
 ***************************************************************************************************/
int safety::SafetyAction::update()
{
    bool cur_state_ = false;
    int res = check_not_safe(cur_state_);
    if (res)
    {
        // #TODO 异常处理
    }
    if (cur_state_ && !last_state_)
    {
        rising_event();
    }
    if (!cur_state_ && last_state_)
    {
        falling_event();
    }
    watch_count_down();
    last_state_ = cur_state_;
    return 0;
}

int safety::SafetyEventWatch::safety_io_watch_frq_ = 125;

SafetyEventWatch& safety::SafetyEventWatch::instance()
{
    static SafetyEventWatch __safety_event_watch;
    return __safety_event_watch;
}

void safety::SafetyEventWatch::init(int frq)
{
    safety_io_watch_frq_ = frq;
    events_.clear();
    events_.push_back(std::make_shared<safety::Cat0Action>());
    events_.push_back(std::make_shared<safety::Cat1Action>());
    events_.push_back(std::make_shared<safety::Cat2Action>());
}

void safety::SafetyEventWatch::watch()
{
    for (auto& e : events_) { e->update(); }
}
